// local_planner.cpp
//
// A ROS Noetic node implementing a TEB-based local planner.
// Subscribes to a global path, local map, and control/reset/cancel topics.
// Uses teb_local_planner and costmap_2d to compute a local trajectory,
// publishes the local path, local costmap, and block feedback as needed.

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <teb_local_planner/teb_local_planner_ros.h>

#include <mutex>

#include <Eigen/Dense> // 需安装 Eigen 库

class FullEKF
{
public:
    FullEKF() : initialized_(false)
    {
        // ROS_INFO("[FullEKF] Initializing...");
        // 初始化状态向量
        x_ = Eigen::VectorXd::Zero(6); // 确保 x_ 大小为 6
        // 初始化状态协方差矩阵 P (6x6)
        P_.resize(6, 6);
        P_.setIdentity();
        // 设置过程噪声协方差 Q (6x6)
        Q_.resize(6, 6);
        Q_.setZero();
        Q_.diagonal() << 0.1, 0.1, 0.1, 0.1, 0.05, 0.05;
        // 设置测量噪声协方差 R (3x3)
        R_.resize(3, 3);
        R_.setZero();
        R_.diagonal() << 0.1, 0.1, 0.05;
        // ROS_INFO("[FullEKF] Initialization complete.");
    }

    void update(const geometry_msgs::PoseStamped &meas_pose)
    {
        double t = meas_pose.header.stamp.toSec();
        if (!initialized_)
        {
            // 初始化状态向量
            x_.head<2>() = Eigen::Vector2d(meas_pose.pose.position.x, meas_pose.pose.position.y);
            x_.segment<2>(2).setZero();                      // 初始速度为零
            x_(4) = tf2::getYaw(meas_pose.pose.orientation); // 初始偏航角
            x_(5) = 0.0;                                     // 初始角速度为零
            last_time_ = t;
            initialized_ = true;
            return;
        }

        double dt = t - last_time_;
        if (dt <= 0.0001)
            return;

        // 1. 预测步骤
        predict(dt);

        // 2. 更新步骤
        Eigen::Vector3d z;
        z(0) = meas_pose.pose.position.x;
        z(1) = meas_pose.pose.position.y;
        z(2) = tf2::getYaw(meas_pose.pose.orientation);
        update(z);

        last_time_ = t;
    }

    double vx() const { return x_(2); }
    double vy() const { return x_(3); }
    double omega() const { return x_(5); }

private:
    void predict(double dt)
    {
        // 状态转移矩阵 F（匀速模型）
        Eigen::MatrixXd F = Eigen::MatrixXd::Identity(6, 6);
        F(0, 2) = dt;
        F(1, 3) = dt;
        F(4, 5) = dt;

        // 预测状态
        x_ = F * x_;

        // 预测协方差
        P_ = F * P_ * F.transpose() + Q_;
    }

    void update(const Eigen::Vector3d &z)
    {
        // 计算测量雅可比矩阵 H（直接观测位置和偏航角）
        Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, 6);
        H(0, 0) = 1.0;
        H(1, 1) = 1.0;
        H(2, 4) = 1.0;

        // 计算卡尔曼增益
        Eigen::MatrixXd S = H * P_ * H.transpose() + R_;
        Eigen::MatrixXd K = P_ * H.transpose() * S.inverse();

        // 计算残差
        Eigen::Vector3d y = z - H * x_;

        // 更新状态和协方差
        x_ += K * y;
        P_ = (Eigen::MatrixXd::Identity(6, 6) - K * H) * P_;
    }

    Eigen::VectorXd x_ = Eigen::VectorXd::Zero(6); // 状态向量
    Eigen::MatrixXd P_;                            // 状态协方差矩阵
    Eigen::MatrixXd Q_;                            // 过程噪声协方差
    Eigen::MatrixXd R_;                            // 测量噪声协方差

    double last_time_;
    bool initialized_;
};

class LocalPlannerNode
{
public:
    LocalPlannerNode()
        : nh_private_("~"),
          tf_buffer_(),
          tf_listener_(tf_buffer_),
          teb_planner_ros_(new teb_local_planner::TebLocalPlannerROS()),
          costmap_ros_("local_costmap", tf_buffer_),
          have_global_plan_(false),
          timer_running_(false),
          blocked_reported_(false)
    {
        ROS_INFO("[LocalPlanner] Node start.");

        // Load parameters
        loadParameters();

        // Initialize components
        initializeComponents();

        ROS_INFO("[LocalPlanner] Node initialized.");
    }

private:
    void loadParameters()
    {
        // 参数加载（移除已废弃的发布相关参数）
        nh_private_.param<std::string>("lp_global_plan_topic", topic_global_plan_, "/global_plan");
        nh_private_.param<std::string>("lp_map_frame_id", map_frame_id_, "map");
        nh_private_.param<std::string>("lp_robot_frame_id", robot_frame_id_, "base_link");
        nh_private_.param<std::string>("lp_cancel_request_topic", topic_cancel_request_, "/cancel_request");
        nh_private_.param<std::string>("lp_reset_request_topic", topic_reset_request_, "/reset_reached");
        nh_private_.param<std::string>("lp_block_respond_topic", topic_block_respond_, "/block_respond");
        nh_private_.param<std::string>("lp_cmd_topic", topic_cmd_vel_, "/cmd_vel");
        nh_private_.param<std::string>("lp_local_map_topic", topic_local_map_, "/local_map");
        nh_private_.param<double>("lp_pose_update_freq", pose_update_freq_, 50.0);
        nh_private_.param<double>("lp_planning_freq", planning_freq_, 20.0);
        nh_private_.param<double>("lp_control_freq", control_freq_, 40.0);
        nh_private_.param<double>("lp_block_patience_time", block_patience_time_, 10.0);
    }

    void initializeComponents()
    {
        // Subscribers
        sub_global_plan_ = nh_.subscribe(topic_global_plan_, 1, &LocalPlannerNode::globalPlanCallback, this);
        sub_cancel_request_ = nh_.subscribe(topic_cancel_request_, 1, &LocalPlannerNode::cancelCallback, this);
        sub_reset_request_ = nh_.subscribe(topic_reset_request_, 1, &LocalPlannerNode::resetCallback, this);
        sub_local_map_ = nh_.subscribe(topic_local_map_, 1, &LocalPlannerNode::localMapCallback, this);

        // Publishers
        pub_block_respond_ = nh_.advertise<std_msgs::String>(topic_block_respond_, 1, true);
        pub_cmd_vel_ = nh_.advertise<geometry_msgs::Twist>(topic_cmd_vel_, 1);

        // Initialize costmap and TEB planner
        costmap_ros_.pause();
        costmap_ros_.start();

        teb_planner_ros_->initialize("teb_local_planner", &tf_buffer_, &costmap_ros_);

        // Initialize timers
        ros::Duration pose_period(1.0 / pose_update_freq_);
        pose_update_timer_ = nh_.createTimer(pose_period, &LocalPlannerNode::poseUpdateTimerCallback, this);
        ros::Duration plan_period(1.0 / planning_freq_);
        plan_timer_ = nh_.createTimer(plan_period, &LocalPlannerNode::planTimerCallback, this);
        ros::Duration control_period(1.0 / control_freq_);
        control_timer_ = nh_.createTimer(control_period, &LocalPlannerNode::controlTimerCallback, this, false, false);
    }

    // --- 定时器回调 ---
    void poseUpdateTimerCallback(const ros::TimerEvent &)
    {
        geometry_msgs::TransformStamped tf_map_to_robot;
        try
        {
            tf_map_to_robot = tf_buffer_.lookupTransform(map_frame_id_, robot_frame_id_,
                                                         ros::Time(0), ros::Duration(0.05));
        }
        catch (tf2::TransformException &ex)
        {
            ROS_WARN_THROTTLE(1.0, "[PoseUpdate] TF lookup failed: %s", ex.what());
            return;
        }

        std::lock_guard<std::mutex> lock(pose_mutex_);
        curr_pose_.header.stamp = ros::Time::now();
        curr_pose_.header.frame_id = map_frame_id_;
        curr_pose_.pose.position.x = tf_map_to_robot.transform.translation.x;
        curr_pose_.pose.position.y = tf_map_to_robot.transform.translation.y;
        curr_pose_.pose.orientation = tf_map_to_robot.transform.rotation;

        ekf_.update(curr_pose_);
    }

    void planTimerCallback(const ros::TimerEvent &)
    {
        std::lock_guard<std::mutex> lock(plan_mutex_);
        if (!have_global_plan_)
            return;

        // 更新全局路径
        teb_planner_ros_->setPlan(global_plan_.poses);

        // 获取位姿用于规划
        geometry_msgs::PoseStamped current_pose;
        {
            std::lock_guard<std::mutex> lock(pose_mutex_);
            current_pose = curr_pose_;
        }

        // 计算速度指令
        if (teb_planner_ros_->computeVelocityCommands(vel_cmd_))
        {
            have_new_plan_ = true;
            last_valid_plan_time_ = ros::Time::now();
        }
        else if ((ros::Time::now() - last_valid_plan_time_).toSec() > 5.0)
        {
            ROS_WARN("[Planner] No valid plan for 5 seconds");
        }
    }

    void controlTimerCallback(const ros::TimerEvent &)
    {
        checkBlocking();

        if (have_new_plan_)
        {
            pub_cmd_vel_.publish(vel_cmd_);
            have_new_plan_ = false;
        }
    }

    void globalPlanCallback(const nav_msgs::Path::ConstPtr &msg)
    {
        std::lock_guard<std::mutex> lock(plan_mutex_);
        global_plan_ = *msg;
        have_global_plan_ = true;
        blocked_reported_ = false;

        if (!timer_running_)
        {
            control_timer_.start();
            timer_running_ = true;
            ROS_INFO("[LocalPlanner] New plan received (%lu poses)", global_plan_.poses.size());
        }
    }

    void cancelCallback(const std_msgs::Empty::ConstPtr &)
    {
        std::lock_guard<std::mutex> lock(plan_mutex_);
        resetPlanner();
        ROS_WARN("[LocalPlanner] Cancel command received");
    }

    void resetCallback(const std_msgs::Empty::ConstPtr &)
    {
        std::lock_guard<std::mutex> lock(plan_mutex_);
        resetPlanner();
        ROS_WARN("[LocalPlanner] Reset command received");
    }

    void checkBlocking()
    {
        // std::lock_guard<std::mutex> lock(pose_mutex_);
        // if (curr_pose_.header.stamp.isZero() || !have_global_plan_)
        //     return;

        // double dx = curr_pose_.pose.position.x - last_moved_pose_.pose.position.x;
        // double dy = curr_pose_.pose.position.y - last_moved_pose_.pose.position.y;
        // double dist_sq = dx * dx + dy * dy;

        // if (dist_sq < 0.1)
        // { // 1cm²阈值
        //     if ((ros::Time::now() - last_moved_time_).toSec() > block_patience_time_ && !blocked_reported_)
        //     {
        //         std::lock_guard<std::mutex> lock(plan_mutex_);
        //         if (global_plan_.poses.size() >= 2)
        //         {
        //             publishBlockResponse(global_plan_.poses[0], global_plan_.poses[1]);
        //             blocked_reported_ = true;
        //         }
        //     }
        // }
        // else
        // {
        //     last_moved_time_ = ros::Time::now();
        //     last_moved_pose_ = curr_pose_;
        //     blocked_reported_ = false;
        // }
    }

    void resetPlanner()
    {
        have_global_plan_ = false;
        global_plan_.poses.clear();
        if (timer_running_)
        {
            control_timer_.stop();
            timer_running_ = false;
        }
    }

    void publishBlockResponse(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2)
    {
        std_msgs::String block_msg;
        std::ostringstream oss;
        oss << p1.pose.position.x << " " << p1.pose.position.y << " "
            << p2.pose.position.x << " " << p2.pose.position.y << " 0 0";
        block_msg.data = oss.str();
        pub_block_respond_.publish(block_msg);
        ROS_WARN("[LocalPlanner] Blocked! Sent response: [%s]", oss.str().c_str());
    }

    void localMapCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg)
    {
        // // 检查 frame_id 是否一致
        // if (msg->header.frame_id != map_frame_id_)
        // {
        //     try
        //     {
        //         // 获取 body -> map 的变换
        //         geometry_msgs::TransformStamped T_map_body = tf_buffer_.lookupTransform(
        //             map_frame_id_, msg->header.frame_id, msg->header.stamp, ros::Duration(0.15));

        //         // 转换 origin
        //         geometry_msgs::PointStamped origin_in_body, origin_in_map;
        //         origin_in_body.header = msg->header;
        //         origin_in_body.point = msg->info.origin.position;
        //         tf2::doTransform(origin_in_body, origin_in_map, T_map_body);

        //         // 获取 costmap 指针
        //         costmap_2d::Costmap2D *costmap = costmap_ros_.getCostmap();
        //         if (!costmap)
        //         {
        //             ROS_WARN("[LocalPlanner] Costmap not initialized");
        //             return;
        //         }

        //         std::lock_guard<std::mutex> lock(costmap_mutex_);

        //         // 调整 costmap 原点
        //         costmap->resizeMap(msg->info.width, msg->info.height, msg->info.resolution,
        //                            origin_in_map.point.x, origin_in_map.point.y);

        //         // 转换并更新数据（考虑旋转）
        //         double yaw = tf2::getYaw(T_map_body.transform.rotation);
        //         double resolution = msg->info.resolution;
        //         for (unsigned int y = 0; y < msg->info.height; ++y)
        //         {
        //             for (unsigned int x = 0; x < msg->info.width; ++x)
        //             {
        //                 int index = y * msg->info.width + x;
        //                 double x_body = x * resolution + msg->info.origin.position.x;
        //                 double y_body = y * resolution + msg->info.origin.position.y;
        //                 double x_map = cos(yaw) * x_body - sin(yaw) * y_body + T_map_body.transform.translation.x;
        //                 double y_map = sin(yaw) * x_body + cos(yaw) * y_body + T_map_body.transform.translation.y;
        //                 int map_x = (x_map - origin_in_map.point.x) / resolution;
        //                 int map_y = (y_map - origin_in_map.point.y) / resolution;
        //                 if (map_x >= 0 && map_x < msg->info.width && map_y >= 0 && map_y < msg->info.height)
        //                 {
        //                     switch (msg->data[index])
        //                     {
        //                     case -1:
        //                         costmap->setCost(map_x, map_y, costmap_2d::NO_INFORMATION);
        //                         break;
        //                     case 0:
        //                         costmap->setCost(map_x, map_y, costmap_2d::FREE_SPACE);
        //                         break;
        //                     default:
        //                         costmap->setCost(map_x, map_y, costmap_2d::LETHAL_OBSTACLE);
        //                         break;
        //                     }
        //                 }
        //             }
        //         }

        //         // 触发 costmap 更新
        //         costmap_ros_.updateMap();
        //     }
        //     catch (tf2::TransformException &ex)
        //     {
        //         ROS_WARN("[LocalPlanner] TF transform failed: %s", ex.what());
        //     }
        // }
        // else
        // {
        //     // frame_id 一致，直接更新
        //     costmap_2d::Costmap2D *costmap = costmap_ros_.getCostmap();
        //     if (!costmap)
        //         return;
        //     std::lock_guard<std::mutex> lock(costmap_mutex_);
        //     costmap->resizeMap(msg->info.width, msg->info.height, msg->info.resolution,
        //                        msg->info.origin.position.x, msg->info.origin.position.y);
        //     // ... 更新数据 ...
        //     costmap_ros_.updateMap();
        // }
    }

    //////////////////////////////////////////////
    // --- 成员变量 ---
    ros::NodeHandle nh_, nh_private_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;

    costmap_2d::Costmap2D costmap_;
    costmap_2d::Costmap2DROS costmap_ros_;
    boost::shared_ptr<teb_local_planner::TebLocalPlannerROS> teb_planner_ros_;

    // Subscribers & Publishers
    ros::Subscriber sub_global_plan_;
    ros::Subscriber sub_cancel_request_;
    ros::Subscriber sub_reset_request_;
    ros::Subscriber sub_local_map_;
    ros::Publisher pub_block_respond_;
    ros::Publisher pub_cmd_vel_;

    // Timers
    ros::Timer pose_update_timer_;
    ros::Timer plan_timer_;
    ros::Timer control_timer_;

    // Parameters
    std::string topic_global_plan_;
    std::string map_frame_id_, robot_frame_id_;
    std::string topic_cancel_request_, topic_reset_request_;
    std::string topic_block_respond_, topic_cmd_vel_;
    std::string topic_local_map_;
    double pose_update_freq_, planning_freq_, control_freq_;
    double block_patience_time_;

    // State
    nav_msgs::Path global_plan_;
    bool have_global_plan_;
    bool timer_running_;
    FullEKF ekf_;
    geometry_msgs::PoseStamped curr_pose_;
    geometry_msgs::PoseStamped last_moved_pose_;
    ros::Time last_moved_time_;
    ros::Time last_valid_plan_time_;
    bool blocked_reported_;

    // 控制指令
    geometry_msgs::Twist vel_cmd_;
    bool have_new_plan_ = false;

    // Mutexes
    std::mutex pose_mutex_;
    std::mutex plan_mutex_;
    std::mutex costmap_mutex_;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "local_planner_node");
    LocalPlannerNode node;
    ros::spin();
    return 0;
}

/*
环境 ubuntu20.04 ros noetic
编写一个 ros noetic 节点代码文件 local_planner.cpp ， 实现一个 TEB 局部规划器，下面是草稿
使用 costmap_2d 和 teb_local_planner 插件
自定义 TEB 局部规划器

工作流：
- 在初始化函数中加载参数，并用参数初始化 TEB 规划器和 costmap_2d 插件
- 接收局部地图，然后利用插件生成代价地图
- 接收到取消导航请求，则清空路径，退出规划模式（关闭timer）
- 接收到重置导航请求，则清空路径，退出规划模式（关闭timer）
- 检测如果在运行时在全局路径中的某一段停留太久，则认为该段被阻塞，发布阻塞反馈，并退出规划模式（关闭timer）
    ps:阻塞反馈字符串内容为六个数字，分别表示停留段两端的坐标值
- 如果接收到全局话题，则保存全局路径到变量中，并进入路径规划模式（打开对应 timer）
- 路径规划模式计时器：
    1. 用一个 EKF 来跟踪机器人的历史位姿，进而计算出速度和角速度
    2. 根据得到的位姿、速度、角速度，以及生成的costmap，以及全局路径，通过TEB优化来优化局部轨迹

*/